#include "includes.h"
#include "drivers.h"
#include "init.h"
#include "data.h"
#include "periph.h"
#include "servo.h"
#include "control.h"
#include "isr.h"

extern led_status led[4];

extern uint16 amp[4];
extern const uint8_t ad_ch[AMP_ADC_CHN+1];

void init_var(void){
}

void pin_mux(void){
    /* motor TPM PWM */                                 // TPM0
    PORTE_PCR24&=~PORT_PCR_MUX_MASK;                    // TPM0_CH0
    PORTE_PCR24|=PORT_PCR_MUX(3);
    PORTE_PCR25&=~PORT_PCR_MUX_MASK;                    // TPM0_CH1
    PORTE_PCR25|=PORT_PCR_MUX(3);
    PORTA_PCR5&=~PORT_PCR_MUX_MASK;                     // TPM0_CH2
    PORTA_PCR5|=PORT_PCR_MUX(3);
    PORTE_PCR30&=~PORT_PCR_MUX_MASK;                    // TPM0_CH3
    PORTE_PCR30|=PORT_PCR_MUX(3);

    /* servo TPM PWM */                                 // TPM1_CH0
    PORTA_PCR12&=~PORT_PCR_MUX_MASK;
    PORTA_PCR12|=PORT_PCR_MUX(3);

    /* TPM impulse counter external clock */            // TPM_CLKIN0
    PORTB_PCR16&=~PORT_PCR_MUX_MASK;
    PORTB_PCR16|=PORT_PCR_MUX(4);

    /* LPTMR0 impulse counter external clock */         // LPTMR0_ALT2
    PORTC_PCR5&=~PORT_PCR_MUX_MASK;
    PORTC_PCR5|=PORT_PCR_MUX(3);
}

void init_gpio(void){
    /* LED port */
    gpio_init(led[0].port, led[0].ch, GPIO_GPO, led[0].val); // On-Chip LED
    gpio_init(led[1].port, led[1].ch, GPIO_GPO, led[1].val); // Red LED
    gpio_init(led[2].port, led[2].ch, GPIO_GPO, led[2].val); // Green LED
    gpio_init(led[3].port, led[3].ch, GPIO_GPO, led[3].val); // Blue LED
    /* input port */
    gpio_init(GPIO_PORTC, 1, GPIO_GPI, 0);
    gpio_init(GPIO_PORTD, 5, GPIO_GPI, 0);
    gpio_init(GPIO_PORTD, 4, GPIO_GPI, 0);
    gpio_init(GPIO_PORTD, 3, GPIO_GPI, 0);
    gpio_init(GPIO_PORTD, 2, GPIO_GPI, 0);
    gpio_init(GPIO_PORTD, 1, GPIO_GPI, 0);
}

void init_isr(void){
    set_irq_priority(12, 1); // UART0 has second priority
    set_irq_priority(22, 0); // PIT has priority of highest
    uart_irq(UART0, UART_IRQ_ENABLE);
}

void init_pwm(void){
    /* Motor PWM */
    tpm_pwm_init(TPM0, TPM0_FREQ_INIT);
    tpm_pwm_duty(TPM0, TPM_CH0, TPM0_CH0_DUTY_INIT); // left forward
    tpm_pwm_duty(TPM0, TPM_CH1, TPM0_CH1_DUTY_INIT); // left backward
    tpm_pwm_duty(TPM0, TPM_CH2, TPM0_CH2_DUTY_INIT); // right backward
    tpm_pwm_duty(TPM0, TPM_CH3, TPM0_CH3_DUTY_INIT); //right forward

    /* Servo PWM */
    tpm_pwm_init(TPM1, TPM1_FREQ_INIT);
    tpm_pwm_duty(TPM1, TPM_CH0, TPM1_CH0_DUTY_INIT);
}

void init_tmr(void){
    /* control routine */
    pit_init_ms(PIT0, PIT0_PERIOD);

    /* System rountine */
    pit_init_ms(PIT1, PIT1_PERIOD);
}

void init_cnt(void){
    /* impulse counter 0 */
    lptmr_init(LPTMR_ALT2, LPTMR_USE_OSCERCLK);

    /* impulse counter 1 */
    tpm_cnt_init(TPM2);
}

void init_dma(void){
    dma_periph_init(DMA_CH0, DMAMUX_ADC0, &ADC0_RA, (uint32_t *)amp, AMP_ADC_CHN*2, DMA_16bit, DMA_16bit, DMA_CYCL_MASK|DMA_DINC_MASK);
    dma_memory_init(DMA_CH1, (uint32_t *)ad_ch, &ADC0_SC1A, AMP_ADC_CHN+1, DMA_8bit, DMA_8bit, DMA_CYCL_MASK|DMA_SINC_MASK);
    DMA_SAR0=(uint32_t)&ADC0_RA;
    DMA_DAR1=(uint32_t)&ADC0_SC1A;
    dma_linkcc(DMA_CH0, 2, DMA_CH1, DMA_CH0);
    dma_irq_enable(DMA_CH1);
    dmamux_enable(DMAMUX_CH0);
    dmamux_enable(DMAMUX_CH1);
    dma_adc_activate();
}

void system_init(void){
    init_var();
    pin_mux();
    init_gpio();
    init_isr();
    init_pwm();
    init_tmr();
    init_cnt();
    init_dma();
}
